#!/usr/bin/python3     
# -*- coding: UTF-8 -*-                    
import rospy
import sqlite3
from math import *
import os
from matplotlib import pyplot as plt
from datetime import datetime
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Point
from std_msgs.msg import Float32
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
# from digital_twin_ws.msg import pur_output
from digital_twin_ws.msg import DTName

# 应用服务类
class ApplicationService:
    timestep = 0.1  # 步长
    time = 0        # 当前秒数
    stoptime = 100   # 结束时间
    ctrl_model = 2  # 控制模式：0-不控；1-只控PE；2-只控VE；3-控PE和VE； 4-rosbag控VE

    PE_name = "PErobot"
    PE_electricity = 0
    PE_x = 0
    PE_y = 0
    PE_th = 0
    PE_vx = 0
    PE_vy = 0
    PE_vth = 0
    VE_x = 3.8
    VE_y = 0.8
    VE_th = 0
    VE_vx = 0
    VE_vy = 0
    VE_vth = 0

    Ref_x =4
    Ref_y =1
    Ref_th =0
    Ref_vx =0
    Ref_vy =0
    Ref_vth =0

    PE_ux = 0
    PE_uy = 0
    PE_uth = 0
    VE_ux = 0
    VE_uy = 0
    VE_uth = 0

    DT_PE_x = 0.0
    DT_PE_dx = 0.0
    DT_PE_y = 0.0
    DT_PE_dy = 0.0
    
    poseflagv = 0
    # PE 的初始位置和角度置为 0 
    pose_flag = 0
    angular_flag = 0
    PE_x0 = 0
    PE_y0 = 0
    PE_th0 = 0

    Start_flag = 0


    # 构造函数
    def __init__(self):
        self._pub = rospy.Publisher('/dt_monitor', DTName, queue_size=10)
        self._PE_velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self._VE_velPub = rospy.Publisher('/controller2', Point, queue_size=10)
        rospy.init_node('ApplicationService', anonymous=True)
        self.Start_flag = int(input("开启控制模式:? 1 or 0 \n"))

        # 每次开启AS时，先清空历史数据库
        # # db_file = '/home/legion/work/DT/catkin_ws/src/digital_twin_ws/scripts/DT_data.db'+ datetime.now().strftime(" %Y-%m-%d %H:%M:%S")
        db_file = '/home/ach/nzx_workspace/src/webots_ros/scripts/DT_data.db'
        if os.path.isfile(db_file):
            os.remove(db_file)
            print("Initialize datebase sucessfully.")
        
        # 连接数据库
        self.conn = sqlite3.connect(db_file)
        self.cursor = self.conn.cursor()

        # 数据表
        self.cursor.execute('''
            create table if not exists DT_states(
                PE_electricity real,   
                PE_x real,
                PE_y real,
                PE_th real,
                PE_vx real,
                PE_vy real,
                PE_vth real,

                VE_x real,
                VE_y real,
                VE_th real,
                VE_vx real,
                VE_vy real,
                VE_vth real,

                Ref_x real,
                Ref_y real,
                Ref_th real,
                Ref_vx real,
                Ref_vy real,
                Ref_vth real,
                
                PE_ux real,
                PE_uy real,
                PE_uth real,
                VE_ux real,
                VE_uy real,
                VE_uth real,
                DT_PE_x real,
                DT_PE_y real
            ) 
        ''')

    # 析构函数关闭数据库
    def __del__( self ):
        # self.VE_vx = 0
        # self.VE_vy = 0
        # self.VE_vth = 0
        # self.PE_velControl()
        # rospy.loginfo("PE stoped!")
        self.DB_start_flag = 0
        self.cursor.close()
        self.conn.close()

    # AS规划的参考轨迹
    def AS_reference(self,num):
        if self.time <= self.stoptime:
            if num == 1:  # 圆轨迹，vy = 0
                self.Ref_vx =0.3
                self.Ref_vth =0.1
                self.Ref_th += self.timestep * self.Ref_vth
                self.Ref_th = self.Limit_angular(self.Ref_th)
                self.Ref_x += self.timestep * self.Ref_vx * cos(self.Ref_th)
                self.Ref_y += self.timestep * self.Ref_vx * sin(self.Ref_th)
                
            if num == 2:  # 圆轨迹，vth = 0
                self.Ref_vx = 0.2
                self.Ref_vy = 0.3

                self.Ref_x = 2 * sin(0.25 * self.time)
                self.Ref_y = 2 * sin(0.5 * self.time)
    
            if num == 3:  # 直线轨迹
                self.Ref_vx = 0.2
                self.Ref_th += self.timestep * self.Ref_vth
                self.Ref_th = self.Limit_angular(self.Ref_th)
                self.Ref_x += self.timestep * self.Ref_vx * cos(self.Ref_th)
                self.Ref_y += self.timestep * self.Ref_vx * sin(self.Ref_th)

            if num == 4:  # 正弦，vth = 0
                self.Ref_vx = 0.2
                self.Ref_vy = 3 * self.Ref_vx * cos(3 * self.Ref_vx * self.time)

                self.Ref_x += self.timestep * self.Ref_vx
                self.Ref_y += self.timestep * self.Ref_vy

            if num == 5:  # 纽带轨迹，vth = 0
                temp_x = self.PE_x0 + 0.2 + 2 * sin(0.15 * self.time)
                temp_y = self.PE_y0 + 0.2 + 2 * sin(0.3 * self.time)

                self.Ref_vx = (temp_x - self.Ref_x) / self.timestep
                self.Ref_vy = (temp_y - self.Ref_y) / self.timestep

                self.Ref_x = temp_x
                self.Ref_y = temp_y
        else:
            self.Ref_x += 0
            self.Ref_y += 0
            self.Ref_th += 0
            

    # AS运行过程
    def process(self):
        rate = rospy.Rate(1/self.timestep)  # 10hz
        # 主循环
        while not rospy.is_shutdown():
            rospy.Subscriber("/PowerVoltage", Float32, self.PE_electricity_callback)
            rospy.Subscriber("/imu", Imu, self.PE_imu_callback)
            rospy.Subscriber("/odom", Odometry, self.PE_odem_callback)
            rospy.Subscriber("/ros_test/odom", Odometry, self.VE_odem_callback)
            rospy.Subscriber("/uwb", Point, self.UWB_callback)

            # 数据库记录数据
            sql = 'insert into DT_states values (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)'
            dtStates = (self.PE_electricity, self.PE_x, self.PE_y, self.PE_th, self.PE_vx, self.PE_vy, self.PE_vth, 
                        self.VE_x, self.VE_y, self.VE_th, self.VE_vx, self.VE_vy, self.VE_vth, 
                        self.Ref_x, self.Ref_y, self.Ref_th, self.Ref_vx, self.Ref_vy, self.Ref_vth,
                        self.PE_ux, self.PE_uy, self.PE_uth, self.VE_ux, self.VE_uy, self.VE_uth,
                        self.DT_PE_x, self.DT_PE_y)

            # 监控状态发布至 'dt_monitor' 的 topic
            msg = DTName()
            msg.header.stamp = rospy.Time.now()
            msg.PE_name = self.PE_name
            msg.PE_electricity = self.PE_electricity
            msg.PE_x = self.PE_x
            msg.PE_y = self.PE_y
            msg.PE_th = self.PE_th
            msg.PE_vx = self.PE_vx
            msg.PE_vy = self.PE_vy
            msg.PE_vth = self.PE_vth

            msg.VE_x = self.VE_x
            msg.VE_y = self.VE_y
            msg.VE_th = self.VE_th
            msg.VE_vx = self.VE_vx
            msg.VE_vy = self.VE_vy
            msg.VE_vth = self.VE_vth
            self._pub.publish(msg)

            # 控制模式：0-不控；1-只控PE；2-只控VE；3-控PE和VE; 4-rosbag控VE
            if self.ctrl_model == 0:
                pass

            # if self.ctrl_model == 1 and self.pose_flag == 0 and self.angular_flag == 0:
            if self.ctrl_model == 1 :
                self.cursor.execute(sql, dtStates)
                self.conn.commit()
                self.AS_reference( 1 )
                self.PE_Controller1()
                # print(self.PE_ux, self.PE_uth)
                self.time += self.timestep

            if self.ctrl_model == 2 :
                self.cursor.execute(sql, dtStates)
                self.conn.commit()
                self.AS_reference( 1 )
                self.VE_independent_control()
                self.time += self.timestep

            # if self.ctrl_model == 3 and (self.pose_flag == 0 or self.angular_flag == 0) and (self.VE_vx != 0 or self.VE_vy != 0 or self.VE_vth != 0):
            if self.ctrl_model == 3 :
                self.cursor.execute(sql, dtStates)
                self.conn.commit()
                self.AS_reference( 5 )
                self.PE_Controller1()
                # self.VE_independent_control()
                self.VE_Controller2()
                self.time += self.timestep

            if self.ctrl_model == 4 and (self.pose_flag == 0 or self.angular_flag == 0) and (self.VE_vx != 0 or self.VE_vy != 0 or self.VE_vth != 0):
                self.cursor.execute(sql, dtStates)
                self.conn.commit()
                self.AS_reference( 5 )
                self.VE_Controller2()
                self.time += self.timestep

            rate.sleep()

    # PE 控制
    def PE_Controller1(self):

        # PE 与 Ref 在车体坐标系下的误差
        PE_ex =  (self.Ref_x - self.PE_x) * cos(self.PE_th) + (self.Ref_y - self.PE_y) * sin(self.PE_th)
        PE_ey = -(self.Ref_x - self.PE_x) * sin(self.PE_th) + (self.Ref_y - self.PE_y) * cos(self.PE_th)
        PE_eth = self.Ref_th - self.PE_th

        # # Controller1
        K1,K2,K3 =0.22, 1.2, 1    #r=0.2  r=0.1
        # K1,K2,K3 =0.24, 1.3, 1.1         #r=0.3
        

        # self.PE_ux = self.Ref_vx * cos(PE_eth) - self.Ref_vy * sin(PE_eth) + K1 * PE_ex
        # self.PE_uy = self.Ref_vx * sin(PE_eth) + self.Ref_vy * cos(PE_eth) + K2 * PE_ey
        # self.PE_uth = self.Ref_vth + K3 * sin(PE_eth)
        
        self.PE_ux = self.Ref_vx * cos(PE_eth) + K1 * PE_ex
        self.PE_uy =0
        self.PE_uth = self.Ref_vth + K2 *self.Ref_vx* PE_ey+K3*self.Ref_vx *sin(PE_eth)
        # self.PE_ux = self.Ref_vx
        # self.PE_uy = self.Ref_vy
        # self.PE_uth = self.Ref_vth

        if self.time > self.stoptime:
            self.PE_ux = 0
            self.PE_uy = 0
            self.PE_uth = 0

        PE_msg = Twist()
        PE_msg.linear.x = self.Limit_value(self.PE_ux, 100)
        PE_msg.linear.y = self.Limit_value(self.PE_uy, 100)
        PE_msg.angular.z = self.Limit_value(self.PE_uth, 5)
        self._PE_velPub.publish(PE_msg)

    # VE 控制
    def VE_Controller2(self):
        
        # self.PE_x = self.DT_r1

        # VE 与 PE 在车体坐标系下的误差
        VE_ex =  (self.PE_x - self.VE_x) * cos(self.VE_th) + (self.PE_y - self.VE_y) * sin(self.VE_th)
        VE_ey = -(self.PE_x - self.VE_x) * sin(self.VE_th) + (self.PE_y - self.VE_y) * cos(self.VE_th)
        VE_eth = self.PE_th - self.VE_th

        # Controller2
        # K4,K5,K6 = 0.11, 0.85, 0.75    #r=0.2
        K4,K5,K6 = 0.08, 0.7, 0.6             #r=0.1
        # K4,K5,K6 = 0.07, 1, 0.85             #r=0.3
        
        # self.VE_ux = self.PE_vx * cos(VE_eth) - self.PE_vy * sin(VE_eth) + K4 * VE_ex
        # self.VE_uy = self.PE_vx * sin(VE_eth) + self.PE_vy * cos(VE_eth) + K5 * VE_ey
        # self.VE_uth = self.PE_vth + K6 * sin(VE_eth)

        self.VE_ux = self.PE_vx * cos(VE_eth) + K4 * VE_ex
        self.VE_uy =0
        self.VE_uth = self.PE_vth + K5 *self.PE_vx* VE_ey+K6*self.PE_vx *sin(VE_eth)

        VE_msg = Point()
        VE_msg.x = self.Limit_value(self.VE_ux, 1)
        VE_msg.y = self.Limit_value(self.VE_uy, 1)
        VE_msg.z = self.Limit_value(self.VE_uth, 1)
        self._VE_velPub.publish(VE_msg)
    
    # VE 独立控制
    def VE_independent_control(self):

        # VE 与 Ref 在车体坐标系下的误差
     #   print(self.Ref_vx,self.Ref_vth, self.Ref_x,self.Ref_y,self.Ref_th)
        VE_ex =  (self.Ref_x - self.VE_x) * cos(self.VE_th) + (self.Ref_y - self.VE_y) * sin(self.VE_th)
        VE_ey = -(self.Ref_x - self.VE_x) * sin(self.VE_th) + (self.Ref_y - self.VE_y) * cos(self.VE_th)
        VE_eth = self.Ref_th - self.VE_th
        # VE_ex =  self.Ref_x - self.VE_x
        # VE_ey = self.Ref_x - self.VE_x
        # VE_eth = self.Ref_th - self.VE_th

        # Controller
        k1,k2,k3 = 0.1,0.3,0.2   # vy=0  圆轨迹最佳参数：n1,n2,n3 = 1.8, 2.5, 2    3, 3, 2.5
        # n1,n2,n3 = 2.5, 2.8, 1.5  # vth=0 双纽线轨迹(0.25,0.5)最佳参数：n1,n2,n3 = 2.5, 2.8, 1.5
        # n1,n2,n3 = 1.1, 1.9, 1.8    # vth=0 双纽线轨迹(0.18,0.36)最佳参数：n1,n2,n3 = 1.1, 1.9, 1.8
        
        self.VE_ux = self.Ref_vx * cos(VE_eth) + k1 * VE_ex
        self.VE_uy =0
        self.VE_uth = self.Ref_vth + k2 * VE_ey+k3*self.Ref_vx *sin(VE_eth)
        # self.VE_ux = self.Ref_vx
        # self.VE_uy =0
        # self.VE_uth = self.Ref_vth
    #     self.VE_ux = self.Ref_vx * cos(VE_eth) + k1 * VE_ex
    #  #   self.VE_uy = self.Ref_vth +  n2*
    #     self.VE_uy =0
    #     self.VE_uth = self.Ref_vth + k2 *self.Ref_vx * VE_ey* cos(VE_eth/2)+k3*sin(VE_eth/2)
    
    
        VE_msg = Point()
        VE_msg.x = self.Limit_value(self.VE_ux, 100)
        VE_msg.y = self.Limit_value(self.VE_uy, 100)
        VE_msg.z = self.Limit_value(self.VE_uth, 50)
        self._VE_velPub.publish(VE_msg)

    # DT 最佳R=11
    def xDT(self,signal):
        R= 11
        self.DT_PE_x += self.timestep * self.DT_PE_dx
        self.DT_PE_dx += self.timestep * (-R*R*(asinh(self.DT_PE_x-signal) + asinh(self.DT_PE_dx/R)))

    def yDT(self,signal):
        R= 11
        self.DT_PE_y += self.timestep * self.DT_PE_dy
        self.DT_PE_dy += self.timestep * (-R*R*(asinh(self.DT_PE_y-signal) + asinh(self.DT_PE_dy/R)))

    # 数值限制函数
    def Limit_value(self,val,lim):
        if val >lim:
            val = lim
        if val < -lim:
            val = -lim
        return val

    # 角度限制在[-180,180]
    def Limit_angular(self,angular):
        if angular >= pi:
            angular -= 2*pi
        if angular <= -pi:
            angular += 2*pi
        return angular

    # 四元数 转 偏航欧拉角
    def orientation_to_yaw(self, w, x , y, z):
        return atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))

    # Callback函数族
    def PE_electricity_callback(self, data):
        self.PE_electricity = data.data

    def PE_imu_callback(self, data):
        w = data.orientation.w
        x = data.orientation.x
        y = data.orientation.y
        z = data.orientation.z
        th = self.orientation_to_yaw(w, x, y, z)

        # if self.angular_flag == 1 and th != 0 :
        #     self.PE_th0 = th
        #     self.Ref_th = self.PE_th0 + 0.2
        #     print("Ref_th0={},PE_th0={}".format(self.Ref_th,self.PE_th0))
        #     self.angular_flag = 0

        self.PE_th = th
        
    def PE_odem_callback(self, data):
        self.PE_vx = data.twist.twist.linear.x
        self.PE_vy = data.twist.twist.linear.y
        self.PE_vth = data.twist.twist.angular.z

    def UWB_callback(self,data):
        # if self.pose_flag == 1 and data.x != 0:
        #     self.DT_PE_x = self.PE_x0 = data.x
        #     self.DT_PE_y = self.PE_y0 = data.y
        #     self.Ref_x = self.PE_x0 + 0.2
        #     self.Ref_y = self.PE_y0 + 0.2 
        #     print("PE的起点:({},{})".format(self.PE_x0,self.PE_y0))
        #     print("参考轨迹的起点:({},{})".format(self.Ref_x,self.Ref_y))
        #     self.pose_flag = 0
        if self.pose_flag == 1:
            self.Ref_x = self.PE_x0
            self.Ref_y = self.PE_y0
            self.PE_x0 = data.x
            self.PE_y0 = data.y
            print("PE的起点:({},{})".format(self.PE_x0,self.PE_y0))
            print("参考轨迹的起点:({},{})".format(self.Ref_x,self.Ref_y))
            self.pose_flag = 0 
        self.PE_x = data.x
        self.PE_y = data.y
        # print("PE的起点:({},{})".format(self.PE_x,self.PE_y))
        # print("参考轨迹的起点:({},{})".format(self.Ref_x,self.Ref_y))
        # self.xDT(self.PE_x)
        # self.yDT(self.PE_y)
        # self.PE_x = self.DT_PE_x
        # self.PE_y = self.DT_PE_y
        
    def VE_odem_callback(self, data):
        # rospy.loginfo("received '/dt_robot/odem'")
        self.VE_x = data.pose.pose.position.x 
        self.VE_y = data.pose.pose.position.y 
        self.VE_vx = data.twist.twist.linear.x
        self.VE_vy = data.twist.twist.linear.y
        self.VE_vth = data.twist.twist.angular.z
        
        if self.poseflagv == 1:
            self.VE_x = self.PE_x0
            self.VE_y = self.PE_y0
            print("VE的起点:({},{})".format(self.PE_x0,self.PE_y0))
            print("参考轨迹的起点:({},{})".format(self.Ref_x,self.Ref_y))
            self.poseflagv = 0
        # Vx = data.pose.pose.orientation.x
        # Vy = data.pose.pose.orientation.y
        # Vz = data.pose.pose.orientation.z
        # Vw = data.pose.pose.orientation.w
        self.VE_th =data.pose.pose.position.z 
        # print ("vep:" .format(self.VE_x,self.VE_y,self.VE_th))


if __name__ == '__main__':
    try:
        AS = ApplicationService()
        AS.process()

    except rospy.ROSInterruptException:
        pass
